#ifndef ARCAMA_HPP
#define ARCAMA_HPP

#include "arcam.hpp"

#include "ARMarkerNFT.h"
#include "trackingSub.h"

class ArCamA : public ArCam
{
public:
    // Modifies globals: kpmHandle, ar2Handle.
    static int initNFT(ARParamLT *&cparamLT, AR_PIXEL_FORMAT pixFormat, KpmHandle *&kpmHandle, AR2HandleT *&ar2Handle)
    {
        ARLOGd("Initialising NFT.\n");
        //
        // NFT init.
        //

        // KPM init.
        kpmHandle = kpmCreateHandle(cparamLT);
        if (!kpmHandle) {
            ARLOGe("Error: kpmCreateHandle.\n");
            return (FALSE);
        }
        //kpmSetProcMode( kpmHandle, KpmProcHalfSize );

        // AR2 init.
        if( (ar2Handle = ar2CreateHandle(cparamLT, pixFormat, AR2_TRACKING_DEFAULT_THREAD_NUM)) == NULL ) {
            ARLOGe("Error: ar2CreateHandle.\n");
            kpmDeleteHandle(&kpmHandle);
            return (FALSE);
        }
        if (threadGetCPU() <= 1) {
            ARLOGi("Using NFT tracking settings for a single CPU.\n");
            ar2SetTrackingThresh(ar2Handle, 5.0);
            ar2SetSimThresh(ar2Handle, 0.50);
            ar2SetSearchFeatureNum(ar2Handle, 16);
            ar2SetSearchSize(ar2Handle, 6);
            ar2SetTemplateSize1(ar2Handle, 6);
            ar2SetTemplateSize2(ar2Handle, 6);
        } else {
            ARLOGi("Using NFT tracking settings for more than one CPU.\n");
            ar2SetTrackingThresh(ar2Handle, 5.0);
            ar2SetSimThresh(ar2Handle, 0.50);
            ar2SetSearchFeatureNum(ar2Handle, 16);
            ar2SetSearchSize(ar2Handle, 12);
            ar2SetTemplateSize1(ar2Handle, 6);
            ar2SetTemplateSize2(ar2Handle, 6);
        }
        // NFT dataset loading will happen later.
        return (TRUE);
    }


    // Modifies globals: threadHandle, surfaceSet[], surfaceSetCount
    static int unloadNFTData(THREAD_HANDLE_T *&threadHandle, AR2SurfaceSetT **surfaceSet, int &surfaceSetCount)
    {
        int i, j;

        if (threadHandle) {
            ARLOGi("Stopping NFT2 tracking thread.\n");
            trackingInitQuit(&threadHandle);
        }
        j = 0;
        for (i = 0; i < surfaceSetCount; i++) {
            if (j == 0) ARLOGi("Unloading NFT tracking surfaces.\n");
            ar2FreeSurfaceSet(&surfaceSet[i]); // Also sets surfaceSet[i] to NULL.
            j++;
        }
        if (j > 0) ARLOGi("Unloaded %d NFT tracking surfaces.\n", j);
        surfaceSetCount = 0;

        return 0;
    }

    // References globals: markersNFTCount
    // Modifies globals: threadHandle, surfaceSet[], surfaceSetCount, markersNFT[]
    static int loadNFTData(THREAD_HANDLE_T *&threadHandle, AR2SurfaceSetT **surfaceSet, int &surfaceSetCount, ARMarkerNFT *&markersNFT, KpmHandle *&kpmHandle,
                           const int markersNFTCount, const int PAGES_MAX=10)
    {
        int i;
        KpmRefDataSet *refDataSet;

        // If data was already loaded, stop KPM tracking thread and unload previously loaded data.
        if (threadHandle) {
            ARLOGi("Reloading NFT data.\n");
            unloadNFTData(threadHandle, surfaceSet, surfaceSetCount);
        } else {
            ARLOGi("Loading NFT data.\n");
        }

        refDataSet = NULL;

        for (i = 0; i < markersNFTCount; i++) {
            // Load KPM data.
            KpmRefDataSet  *refDataSet2;
            ARLOGi("Reading %s.fset3\n", markersNFT[i].datasetPathname);
            if (kpmLoadRefDataSet(markersNFT[i].datasetPathname, "fset3", &refDataSet2) < 0 ) {
                ARLOGe("Error reading KPM data from %s.fset3\n", markersNFT[i].datasetPathname);
                markersNFT[i].pageNo = -1;
                continue;
            }
            markersNFT[i].pageNo = surfaceSetCount;
            ARLOGi("  Assigned page no. %d.\n", surfaceSetCount);
            if (kpmChangePageNoOfRefDataSet(refDataSet2, KpmChangePageNoAllPages, surfaceSetCount) < 0) {
                ARLOGe("Error: kpmChangePageNoOfRefDataSet\n");
                exit(-1);
            }
            if (kpmMergeRefDataSet(&refDataSet, &refDataSet2) < 0) {
                ARLOGe("Error: kpmMergeRefDataSet\n");
                exit(-1);
            }
            ARLOGi("  Done.\n");

            // Load AR2 data.
            ARLOGi("Reading %s.fset\n", markersNFT[i].datasetPathname);

            if ((surfaceSet[surfaceSetCount] = ar2ReadSurfaceSet(markersNFT[i].datasetPathname, "fset", NULL)) == NULL ) {
                ARLOGe("Error reading data from %s.fset\n", markersNFT[i].datasetPathname);
            }
            ARLOGi("  Done.\n");

            surfaceSetCount++;
            if (surfaceSetCount == PAGES_MAX) break;
        }
        if (kpmSetRefDataSet(kpmHandle, refDataSet) < 0) {
            ARLOGe("Error: kpmSetRefDataSet\n");
            exit(-1);
        }
        kpmDeleteRefDataSet(&refDataSet);

        // Start the KPM tracking thread.
        threadHandle = trackingInitInit(kpmHandle);
        if (!threadHandle) exit(-1);

        ARLOGi("Loading of NFT data complete.\n");
        return (TRUE);
    }

    static void markerDetection(THREAD_HANDLE_T *&threadHandle, int &detectedPage, AR2VideoBufferT *&image, float trackingTrans[3][4],
    AR2HandleT *&ar2Handle, ARMarkerNFT *&markersNFT, AR2SurfaceSetT **surfaceSet,
    const int markersNFTCount, const int surfaceSetCount, const ARdouble VIEW_SCALEFACTOR=1.0)
    {

        int             i, j, k;
        // Run marker detection on frame
        if (threadHandle) {
            // Perform NFT tracking.
            float            err;
            int              ret;
            int              pageNo;

            if( detectedPage == -2 ) {
                trackingInitStart( threadHandle, image->buffLuma );
                detectedPage = -1;
            }
            if( detectedPage == -1 ) {
                ret = trackingInitGetResult( threadHandle, trackingTrans, &pageNo);
                if( ret == 1 ) {
                    if (pageNo >= 0 && pageNo < surfaceSetCount) {
                        ARLOGd("Detected page %d.\n", pageNo);
                        detectedPage = pageNo;
                        ar2SetInitTrans(surfaceSet[detectedPage], trackingTrans);
                    } else {
                        ARLOGe("Detected bad page %d.\n", pageNo);
                        detectedPage = -2;
                    }
                } else if( ret < 0 ) {
                    ARLOGd("No page detected.\n");
                    detectedPage = -2;
                }
            }
            if( detectedPage >= 0 && detectedPage < surfaceSetCount) {
                if( ar2Tracking(ar2Handle, surfaceSet[detectedPage], image->buff, trackingTrans, &err) < 0 ) {
                    ARLOGd("Tracking lost.\n");
                    detectedPage = -2;
                } else {
                    ARLOGd("Tracked page %d (max %d).\n", detectedPage, surfaceSetCount - 1);
                }
            }
        } else {
            ARLOGe("Error: threadHandle\n");
            detectedPage = -2;
        }

        // Update markers.
        for (i = 0; i < markersNFTCount; i++) {
            markersNFT[i].validPrev = markersNFT[i].valid;
            if (markersNFT[i].pageNo >= 0 && markersNFT[i].pageNo == detectedPage) {
                markersNFT[i].valid = TRUE;
                for (j = 0; j < 3; j++) for (k = 0; k < 4; k++) markersNFT[i].trans[j][k] = trackingTrans[j][k];
            }
            else markersNFT[i].valid = FALSE;
            if (markersNFT[i].valid) {

                // Filter the pose estimate.
                if (markersNFT[i].ftmi) {
                    if (arFilterTransMat(markersNFT[i].ftmi, markersNFT[i].trans, !markersNFT[i].validPrev) < 0) {
                        ARLOGe("arFilterTransMat error with marker %d.\n", i);
                    }
                }

                if (!markersNFT[i].validPrev) {
                    // Marker has become visible, tell any dependent objects.
                    // --->
                }

                // We have a new pose, so set that.
                arglCameraViewRH((const ARdouble (*)[4])markersNFT[i].trans, markersNFT[i].pose.T, VIEW_SCALEFACTOR);
                // Tell any dependent objects about the update.
                // --->

            } else {

                if (markersNFT[i].validPrev) {
                    // Marker has ceased to be visible, tell any dependent objects.
                    // --->
                }
            }
        }
    }

public:
    const int PAGES_MAX=10;

    // Markers.
    ARMarkerNFT *markersNFT;
    int markersNFTCount;

    // NFT.
    THREAD_HANDLE_T     *threadHandle;
    AR2HandleT          *ar2Handle;
    KpmHandle           *kpmHandle;
    int                  surfaceSetCount;
    AR2SurfaceSetT      *surfaceSet[10];
//    long                 gCallCountMarkerDetect;

    int detectedPage;
    float trackingTrans[3][4];

public:
    ArCamA()
        : ArCam()
        , markersNFT(NULL)
        , markersNFTCount(0)
        , threadHandle(NULL)
        , ar2Handle(NULL)
        , kpmHandle(NULL)
        , surfaceSetCount(0)
//        , gCallCountMarkerDetect(0)
    {

//        surfaceSet=new AR2SurfaceSetT*[PAGES_MAX];
    }


    void End()
    {
        if (markersNFT) deleteMarkers(&markersNFT, &markersNFTCount);

        // NFT cleanup.
        unloadNFTData(threadHandle, surfaceSet, surfaceSetCount);

        ARLOGd("Cleaning up ARToolKit NFT handles.\n");
        ar2DeleteHandle(&ar2Handle);
        kpmDeleteHandle(&kpmHandle);

        ArCam::End();
    }

    int Start(const char markerConfigDataFilename[] = "../share/artoolkit-examples/Data2/markers.dat")
    {
        if(ArCam::Start()==FALSE)
            return FALSE;

//        const char markerConfigDataFilename[] = "../share/artoolkit-examples/Data2/markers.dat";

        if (!initNFT(gCparamLT, arVideoGetPixelFormat(), kpmHandle, ar2Handle)) {
            ARLOGe("main(): Unable to init NFT.\n");
            exit(-1);
        }

        // Load marker(s).
        newMarkers(markerConfigDataFilename, &markersNFT, &markersNFTCount);
        if (!markersNFTCount) {
            ARLOGe("Error loading markers from config. file '%s'.\n", markerConfigDataFilename);
            End();
            exit(-1);
        }
        ARLOGi("Marker count = %d\n", markersNFTCount);

        // Marker data has been loaded, so now load NFT data.
        if (!loadNFTData(threadHandle, surfaceSet, surfaceSetCount, markersNFT, kpmHandle, markersNFTCount, PAGES_MAX)) {
            ARLOGe("Error loading NFT data.\n");
            End();
            exit(-1);
        }
        return TRUE;
    }


    bool measure(cv::Vec3d &point, cv::Vec2d pointInPlanemm)
    {
        std::cout<<"measure\n"<<std::flush;

        AR2VideoBufferT *image;
        // Grab a video frame.
        image = arVideoGetImage();
        if (image && image->fillFlag) {


            markerDetection(threadHandle, detectedPage, image, trackingTrans,
                    ar2Handle, markersNFT, surfaceSet,
                    markersNFTCount, surfaceSetCount);

            int i;

            for (i = 0; i < markersNFTCount; i++) {

                if (markersNFT[i].valid) {

//                    world coordinate of marker center W={W[0],W[1],W[2],W[3]}
         //transform matrix T={T[0],T[4],T[8],T[12]
         //                    T[1],T[5],T[9],T[13]
         //                    T[2],T[6],T[10],T[14]
         //                    T[3],T[7],T[11],T[15]}
         //        marker cooridnate of center C={C[0],C[1],C[2],C[3]}
         //                                      W=T*C

                            point=cv::Vec3d(
                             markersNFT[i].pose.T[12]+pointInPlanemm[0]*markersNFT[i].pose.T[0]+pointInPlanemm[1]*markersNFT[i].pose.T[4],
                             markersNFT[i].pose.T[13]+pointInPlanemm[0]*markersNFT[i].pose.T[1]+pointInPlanemm[1]*markersNFT[i].pose.T[5],
                             -(markersNFT[i].pose.T[14]+pointInPlanemm[0]*markersNFT[i].pose.T[2]+pointInPlanemm[1]*markersNFT[i].pose.T[6])
                             );
                    return true;

                }
            }

        }
        return false;
    }


    void test(double scale=1, double cxmm=149.3013001696, double cymm=73.2063312606, const char markerConfigDataFilename[] = "../share/artoolkit-examples/Data2/markers.dat")
    {
        int k=0;

        cv::namedWindow("m");

        k=cv::waitKey(30000);



        if(Start(markerConfigDataFilename)==FALSE)
            return;

        FILE *file = NULL;


        file=fopen("/dev/ttyACM0","w");

        cv::Vec3d pw;
        cv::Vec2d pp(cxmm,cymm);

        detectedPage=-2;

        while(k!=27)
//        for(int i=0;i<200;i++)
        {
            if(measure(pw, pp))
            {

                pw=scale*pw;

                if(file!=NULL)
                            {
                                fprintf(file, "%g,%g,%g,",pw[0], pw[1], pw[2]);
                                fflush(file);
                            }
                            else
                            {
                                printf("null file\n");
                            }

//                std::cout<<i<<"\tfound="<<pw<<"\n";
                std::cout<<"\tfound="<<pw<<"\n";



            }
            else
            {
                if(file!=NULL)
                            {
                                fprintf(file, "0,0,-1,");
                                fflush(file);
                            }
                            else
                            {
                                printf("null file\n");
                            }
//                std::cout<<i<<"\tnot found\n";
                std::cout<<"\tnot found\n";

            }
            k=cv::waitKey(1);
        }
        End();

        printf("file close\n");
        fclose(file);

    }



};


#endif // ARCAMA_HPP

